// Copyright (c) 2024, lfr
// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

//
// Source of this file are templates in
// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository.
//

#include "lqr_vmc_controller/lqr_vmc_controller.hpp"

#include <limits>
#include <memory>
#include <string>
#include <vector>
#include <utility>
#include <chrono>
#include <array>

#include "controller_interface/helpers.hpp"

#include "lifecycle_msgs/msg/state.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "rclcpp/logging.hpp"
#include "tf2/LinearMath/Matrix3x3.h"
#include <Eigen/Dense>
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

namespace
{  // utility

// TODO(destogl): remove this when merged upstream
// Changed services history QoS to keep all so we don't lose any client service calls
static constexpr rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = {
  RMW_QOS_POLICY_HISTORY_KEEP_ALL,
  1,  // message queue depth
  RMW_QOS_POLICY_RELIABILITY_RELIABLE,
  RMW_QOS_POLICY_DURABILITY_VOLATILE,
  RMW_QOS_DEADLINE_DEFAULT,
  RMW_QOS_LIFESPAN_DEFAULT,
  RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
  RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, 
  false};

using ControllerReferenceMsg = lqr_vmc_controller::LqrVmcController::ControllerReferenceMsg;
using lifecycle_msgs::msg::State;
using namespace std::chrono_literals;
using SetParametersResult = rcl_interfaces::msg::SetParametersResult; // 用于thigh_angle_d参数设置，实时任意修改机器人高度

// called from RT control loop  // 重置初始化ControllerReferenceMsg的函数
// void reset_controller_reference_msg(
//   std::shared_ptr<ControllerReferenceMsg> & msg)
// {
//   // msg->joint_names = joint_names; 
//   // msg->displacements.resize(joint_names.size(), std::numeric_limits<double>::quiet_NaN());
//   // msg->velocities.resize(joint_names.size(), std::numeric_limits<double>::quiet_NaN());
//   // msg->duration = std::numeric_limits<double>::quiet_NaN();

//   msg->twist.linear.x = 0.0; 
//   msg->twist.linear.y = 0.0;
//   msg->twist.linear.z = 0.0;
//   msg->twist.angular.x = 0.0;
//   msg->twist.angular.y = 0.0;
//   msg->twist.angular.z = 0.0;

// }

}  // namespace

namespace lqr_vmc_controller
{

using hardware_interface::HW_IF_POSITION;
using hardware_interface::HW_IF_VELOCITY;

LqrVmcController::LqrVmcController() : controller_interface::ControllerInterface() {}

CallbackReturn LqrVmcController::on_init()
{
  control_mode_.initRT(control_mode_type::RUNNING);

  try
  {
    // 声明参数
    auto_declare<std::vector<std::string>>("joints", std::vector<std::string>());
    auto_declare<std::vector<std::string>>("state_joints", std::vector<std::string>());
    auto_declare<std::vector<std::string>>("command_interfaces", std::vector<std::string>());
    auto_declare<std::vector<std::string>>("state_interfaces", std::vector<std::string>());
    auto_declare<double>("wheel_separation", params_.wheel_separation);
    auto_declare<double>("wheel_radius", params_.wheel_radius);
    auto_declare<double>("wheel_separation_multiplier", params_.wheel_separation_multiplier);
    auto_declare<double>("left_wheel_radius_multiplier", params_.left_wheel_radius_multiplier);
    auto_declare<double>("right_wheel_radius_multiplier", params_.right_wheel_radius_multiplier);
    auto_declare<bool>("tf_frame_prefix_enable", params_.tf_frame_prefix_enable);
    auto_declare<std::string>("tf_frame_prefix", params_.tf_frame_prefix);
    auto_declare<std::string>("odom_frame_id", params_.odom_frame_id);
    auto_declare<std::string>("base_frame_id", params_.base_frame_id);
    auto_declare<std::vector<double>>("pose_covariance_diagonal", std::vector<double>());
    auto_declare<std::vector<double>>("twist_covariance_diagonal", std::vector<double>());
    auto_declare<double>("cmd_vel_timeout", params_.cmd_vel_timeout);
    auto_declare<int>("velocity_rolling_window_size", params_.velocity_rolling_window_size);
    auto_declare<double>("publish_rate", params_.publish_rate);
    auto_declare<std::string>("sensor_name", params_.sensor_name);
    auto_declare<std::string>("sensor_frame_id", params_.sensor_frame_id);
    auto_declare<std::vector<double>>("static_covariance_orientation", std::vector<double>());
    auto_declare<std::vector<double>>("static_covariance_angular_velocity", std::vector<double>());
    auto_declare<std::vector<double>>("static_covariance_linear_acceleration", std::vector<double>());
  }
  catch (const std::exception & e)
  {
    fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what());
    return CallbackReturn::ERROR;
  }

  return CallbackReturn::SUCCESS;
}

bool LqrVmcController::reset()
{
  odometry_.resetOdometry();

  subscriber_is_active_ = false;
  // 订阅话题reset
  ref_subscriber_.reset();
  imu_subscriber_.reset();
  height_cmd_subscriber_.reset();
  roll_cmd_subscriber_.reset();

  received_velocity_msg_ptr_.set(nullptr);
  received_imu_msg_ptr_.set(nullptr);
  received_height_cmd_msg_ptr_.set(nullptr);
  received_roll_cmd_msg_ptr_.set(nullptr);
  is_halted = false;
  return true;
}

CallbackReturn LqrVmcController::on_configure(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  // 获取参数
  params_.joints = node_->get_parameter("joints").as_string_array();
  params_.state_joints = node_->get_parameter("state_joints").as_string_array();
  params_.command_interfaces = node_->get_parameter("command_interfaces").as_string_array();
  params_.state_interfaces = node_->get_parameter("state_interfaces").as_string_array();
  params_.wheel_separation = node_->get_parameter("wheel_separation").as_double();
  params_.wheel_radius = node_->get_parameter("wheel_radius").as_double();
  params_.wheel_separation_multiplier = node_->get_parameter("wheel_separation_multiplier").as_double();
  params_.left_wheel_radius_multiplier = node_->get_parameter("left_wheel_radius_multiplier").as_double();
  params_.right_wheel_radius_multiplier = node_->get_parameter("right_wheel_radius_multiplier").as_double();
  params_.tf_frame_prefix_enable = node_->get_parameter("tf_frame_prefix_enable").as_bool();
  params_.tf_frame_prefix = node_->get_parameter("tf_frame_prefix").as_string();
  params_.odom_frame_id = node_->get_parameter("odom_frame_id").as_string();
  params_.base_frame_id = node_->get_parameter("base_frame_id").as_string();
  auto pose_covariance_diagonal = node_->get_parameter("pose_covariance_diagonal").as_double_array();
  std::copy(pose_covariance_diagonal.begin(), pose_covariance_diagonal.end(), params_.pose_covariance_diagonal.begin());
  auto twist_covariance_diagonal = node_->get_parameter("twist_covariance_diagonal").as_double_array();
  std::copy(twist_covariance_diagonal.begin(), twist_covariance_diagonal.end(), params_.twist_covariance_diagonal.begin());
  params_.cmd_vel_timeout = node_->get_parameter("cmd_vel_timeout").as_double();
  params_.velocity_rolling_window_size = node_->get_parameter("velocity_rolling_window_size").as_int();
  params_.publish_rate = node_->get_parameter("publish_rate").as_double();
  params_.sensor_name = node_->get_parameter("sensor_name").as_string();
  params_.sensor_frame_id = node_->get_parameter("sensor_frame_id").as_string();
  auto static_covariance_orientation = node_->get_parameter("static_covariance_orientation").as_double_array();
  std::copy(static_covariance_orientation.begin(), static_covariance_orientation.end(), params_.static_covariance_orientation.begin());
  auto static_covariance_angular_velocity = node_->get_parameter("static_covariance_angular_velocity").as_double_array();
  std::copy(static_covariance_angular_velocity.begin(), static_covariance_angular_velocity.end(), params_.static_covariance_angular_velocity.begin());
  auto static_covariance_linear_acceleration = node_->get_parameter("static_covariance_linear_acceleration").as_double_array();
  std::copy(static_covariance_linear_acceleration.begin(), static_covariance_linear_acceleration.end(), params_.static_covariance_linear_acceleration.begin());

  if (!params_.state_joints.empty())
  {
    state_joints_ = params_.state_joints;
  }
  else
  {
    state_joints_ = params_.joints;
  }

  if (params_.joints.size() != state_joints_.size())
  {
    RCLCPP_FATAL(
      get_node()->get_logger(),
      "Size of 'joints' (%zu) and 'state_joints' (%zu) parameters has to be the same!",
      params_.joints.size(), state_joints_.size());
    return CallbackReturn::FAILURE;
  }

  // topics QoS
  auto subscribers_qos = rclcpp::SystemDefaultsQoS();
  subscribers_qos.keep_last(1);
  subscribers_qos.best_effort();

  // 配置odom参数
  const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation;
  const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius;
  const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius;

  odometry_.setWheelParams(wheel_separation, left_wheel_radius, right_wheel_radius);
  odometry_.setVelocityRollingWindowSize(params_.velocity_rolling_window_size);

  // 将参数中以秒为单位的超时时间改为毫秒单位
  cmd_vel_timeout_ = std::chrono::milliseconds{static_cast<int>(params_.cmd_vel_timeout * 1000.0)};

  if (!reset())
  {
    return CallbackReturn::ERROR;
  }

  // 创建订阅上层话题的空消息
  const ControllerReferenceMsg empty_twist;
  received_velocity_msg_ptr_.set(std::make_shared<ControllerReferenceMsg>(empty_twist));
  const sensor_msgs::msg::Imu empty_imu_msg;
  received_imu_msg_ptr_.set(std::make_shared<sensor_msgs::msg::Imu>(empty_imu_msg));
  const std_msgs::msg::Bool empty_height_cmd_msg;
  received_height_cmd_msg_ptr_.set(std::make_shared<std_msgs::msg::Bool>(empty_height_cmd_msg));
  const std_msgs::msg::Bool empty_roll_cmd_msg;
  received_roll_cmd_msg_ptr_.set(std::make_shared<std_msgs::msg::Bool>(empty_roll_cmd_msg));

  // Reference Subscriber  启动上层应用的订阅
  ref_subscriber_ = get_node()->create_subscription<ControllerReferenceMsg>(
    "~/cmd_vel", subscribers_qos,
    std::bind(&LqrVmcController::reference_callback, this, std::placeholders::_1));
  // 启动imu的订阅
  imu_subscriber_ = get_node()->create_subscription<sensor_msgs::msg::Imu>(
    "/imu", subscribers_qos, 
    std::bind(&LqrVmcController::imu_callback, this, std::placeholders::_1)
  );
  // 启动手柄对高度调节命令发布的订阅（后面也可以通过参数通信来任意改变高度）
  height_cmd_subscriber_ = get_node()->create_subscription<std_msgs::msg::Bool>(
    "~/height_cmd", subscribers_qos,
    std::bind(&LqrVmcController::height_cmd_callback, this, std::placeholders::_1)
  );
  // 启动手柄对roll姿态调节命令发布的订阅
  roll_cmd_subscriber_ = get_node()->create_subscription<std_msgs::msg::Bool>(
    "~/roll_cmd", subscribers_qos,
    std::bind(&LqrVmcController::roll_cmd_callback, this, std::placeholders::_1)
  );

  // 设置控制模式的服务回调函数
  auto set_mode_service_callback =
    [&](
      const std::shared_ptr<ControllerModeSrvType::Request> request,
      std::shared_ptr<ControllerModeSrvType::Response> response) 
  {
    if (request->data) 
    {
      control_mode_.writeFromNonRT(control_mode_type::RUNNING);
    }
    else
    {
      control_mode_.writeFromNonRT(control_mode_type::STOP);
    }
    response->success = true;
  };

  // 创建改变控制模式的服务
  set_control_mode_service_ = get_node()->create_service<ControllerModeSrvType>(
    "~/set_control_mode", set_mode_service_callback,
    rmw_qos_profile_services_hist_keep_all);

  try
  {
    // State publisher 创建控制器状态发布者 Odom
    s_publisher_ =
      get_node()->create_publisher<ControllerStateMsg>("~/odom", rclcpp::SystemDefaultsQoS());
    state_publisher_ = std::make_unique<ControllerStatePublisher>(s_publisher_);

    // 创建tf状态发布者
    odometry_transform_publisher_ = get_node()->create_publisher<tf2_msgs::msg::TFMessage>(
    "/tf", rclcpp::SystemDefaultsQoS());
    realtime_odometry_transform_publisher_ =
    std::make_shared<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>>(
      odometry_transform_publisher_);
    
    // 创建debug数据发布者
    debug_publisher_ = get_node()->create_publisher<debug_msgs::msg::DebugData>("~/debug", rclcpp::SystemDefaultsQoS());
    realtime_debug_publisher_ = std::make_shared<realtime_tools::RealtimePublisher<debug_msgs::msg::DebugData>>(debug_publisher_);

  }
  catch (const std::exception & e)  
  {
    fprintf(
      stderr, "Exception thrown during publisher creation at configure stage with message : %s \n",
      e.what());
    return CallbackReturn::ERROR;
  }

  // Append the tf prefix if there is one
  std::string tf_prefix = "";
  if (params_.tf_frame_prefix_enable)
  {
    if (params_.tf_frame_prefix != "")
    {
      tf_prefix = params_.tf_frame_prefix;
    }
    else
    {
      tf_prefix = std::string(get_node()->get_namespace());
    }

    if (tf_prefix == "/")
    {
      tf_prefix = "";
    }
    else
    {
      tf_prefix = tf_prefix + "/";
    }
  }

  // TODO(anyone): Reserve memory in state publisher depending on the message type
  // odom发布初始化
  state_publisher_->lock();
  state_publisher_->msg_.header.frame_id = tf_prefix + params_.odom_frame_id; 
  state_publisher_->msg_.child_frame_id = tf_prefix + params_.base_frame_id;
  state_publisher_->msg_.twist = geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL);
  constexpr size_t NUM_DIMENSIONS = 6;
  for (size_t index = 0; index < 6; ++index)
  {
    // 0, 7, 14, 21, 28, 35
    const size_t diagonal_index = NUM_DIMENSIONS * index + index;
    state_publisher_->msg_.pose.covariance[diagonal_index] = params_.pose_covariance_diagonal[index];
    state_publisher_->msg_.twist.covariance[diagonal_index] = params_.twist_covariance_diagonal[index];
  }
  state_publisher_->unlock();

  // keeping track of odom and base_link transforms only
  realtime_odometry_transform_publisher_->lock();
  auto & odometry_transform_message = realtime_odometry_transform_publisher_->msg_;
  odometry_transform_message.transforms.resize(1);
  odometry_transform_message.transforms.front().header.frame_id = tf_prefix + params_.odom_frame_id;
  odometry_transform_message.transforms.front().child_frame_id = tf_prefix + params_.base_frame_id;
  realtime_odometry_transform_publisher_->unlock();

  // debug数据发布初始化
  realtime_debug_publisher_->lock();
  auto & debug_msg = realtime_debug_publisher_->msg_;
  debug_msg.stamp = get_node()->get_clock()->now();
  realtime_debug_publisher_->unlock();

  // limit the publication on the topics /odom and /tf
  publish_rate_ = params_.publish_rate;
  publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_);

  previous_update_timestamp_ = get_node()->get_clock()->now();

  // 声明和获取参数初始值
  get_node()->declare_parameter("thigh_angle_d", 8.0*M_PI/180.0);
  get_node()->get_parameter("thigh_angle_d", thigh_angle_d_);
  // 接受参数更新
  parameters_callback_handle_ = get_node()->add_on_set_parameters_callback([&](const std::vector<rclcpp::Parameter> &parameters)->SetParametersResult
  {
    // 遍历参数
    for (auto param : parameters)
    {
      RCLCPP_INFO(get_node()->get_logger(),"更新参数值: %s = %f",param.get_name().c_str(),param.as_double());
      if (param.get_name() == "thigh_angle_d")
      { 
        double last_thigh_angle_d = thigh_angle_d_;
        thigh_angle_d_ = param.as_double();
        thighangle_fivethplanner_.getfivethParam(last_thigh_angle_d, 0, 0, thigh_angle_d_, 0, 0, 2, updata_rate_);
        thighangle_fivethplanner_.startplanner();
      }
    }
    auto result = SetParametersResult();
    result.successful = true;
    return result;
  });

  RCLCPP_INFO(get_node()->get_logger(), "configure successful");
  return CallbackReturn::SUCCESS;
}

// 接受上层速度指令的回调函数
void LqrVmcController::reference_callback(const std::shared_ptr<ControllerReferenceMsg> msg)
{
  // if (msg->joint_names.size() == params_.joints.size())
  // {
  // input_ref_.writeFromNonRT(msg); 
  // }
  // else
  // {
  //   RCLCPP_ERROR(
  //     get_node()->get_logger(),
  //     "Received %zu , but expected %zu joints in command. Ignoring message.",
  //     msg->joint_names.size(), params_.joints.size());
  // }

  if (!subscriber_is_active_)
  {
    RCLCPP_WARN(get_node()->get_logger(), "Can't accept new commands. subscriber is inactive");
    return;
  }
  // if (msg->header.frame_id != params_.base_frame_id)
  // {
  //   RCLCPP_ERROR(get_node()->get_logger(), "上层应用发布的twist消息的base_frame_id不是需要的base_frame_id");
  //   return;
  // }
  if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0))
  {
    RCLCPP_WARN_ONCE(
    get_node()->get_logger(),
    "Received TwistStamped with zero timestamp, setting it to current "
    "time, this message will only be shown once");
    msg->header.stamp = get_node()->get_clock()->now();
  }
  // 记录接受到上层消息的时间
  msg->header.stamp = get_node()->get_clock()->now();
  received_velocity_msg_ptr_.set(std::move(msg));

}

// 接收imu话题的回调函数
void LqrVmcController::imu_callback(const std::shared_ptr<sensor_msgs::msg::Imu> imu_msg)
{
  // if (!subscriber_is_active_)
  // {
  //   RCLCPP_WARN(get_node()->get_logger(), "Can't accept imu msgs. subscriber is inactive");
  //   return;
  // }
  received_imu_msg_ptr_.set(std::move(imu_msg));
}

// 接收手柄height_cmd话题的回调函数
void LqrVmcController::height_cmd_callback(const std::shared_ptr<std_msgs::msg::Bool> height_cmd_msg)
{
  if (!subscriber_is_active_)
  {
    RCLCPP_WARN(get_node()->get_logger(), "Can't accept height cmd msgs. subscriber is inactive");
    return;
  }
  received_height_cmd_msg_ptr_.set(std::move(height_cmd_msg));
  // 改变高度的档位
  std::shared_ptr<std_msgs::msg::Bool> height_msg_value;
  received_height_cmd_msg_ptr_.get(height_msg_value);
  if (height_msg_value->data == true)
  {
    // 档位升高，但不能超过最高档
    if (now_pos_ < 4)
    {
      now_pos_++;
    }
  }
  else if (height_msg_value->data == false)
  {
    // 档位降低， 但不能低于最低档
    if (now_pos_ > 0)
    {
      now_pos_--;
    }
  }
  // 根据档位改变高度期望
  double last_thigh_angle_d = thigh_angle_d_; // p0为旧期望
  thigh_angle_d_ = height_joy_pos_[now_pos_];
  thighangle_fivethplanner_.getfivethParam(last_thigh_angle_d, 0, 0, thigh_angle_d_, 0, 0, 2, updata_rate_);
  thighangle_fivethplanner_.startplanner();
}

// 接收roll_cmd话题的回调函数
void LqrVmcController::roll_cmd_callback(const std::shared_ptr<std_msgs::msg::Bool> roll_cmd_msg)
{
  if (!subscriber_is_active_)
  {
    RCLCPP_WARN(get_node()->get_logger(), "Can't accept height cmd msgs. subscriber is inactive");
    return;
  }
  received_roll_cmd_msg_ptr_.set(std::move(roll_cmd_msg));
  // 改变roll姿态角的期望值
  std::shared_ptr<std_msgs::msg::Bool> roll_msg_value;
  received_roll_cmd_msg_ptr_.get(roll_msg_value);
  // 记录旧期望roll角度
  double last_roll_angle_d = roll_angle_d_;
  if (roll_msg_value->data == true) // 按下左倾斜键
  {
    if (roll_angle_d_ == roll_joy_pos_)
    {
      roll_angle_d_ = 0.0;
    }
    else if (roll_angle_d_ == 0.0)
    {
      roll_angle_d_ = - roll_joy_pos_;
    }
  }
  else if (roll_msg_value->data == false) // 按下右倾斜键
  {
    if (roll_angle_d_ == - roll_joy_pos_)
    {
      roll_angle_d_ = 0.0;
    }
    else if (roll_angle_d_ == 0.0)
    {
      roll_angle_d_ = roll_joy_pos_;
    }
  }
  rollangle_fivethplanner_.getfivethParam(last_roll_angle_d, 0, 0, roll_angle_d_, 0, 0, 2, updata_rate_);
  rollangle_fivethplanner_.startplanner();

}

controller_interface::InterfaceConfiguration LqrVmcController::command_interface_configuration() const
{
  controller_interface::InterfaceConfiguration command_interfaces_config;
  command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;

  command_interfaces_config.names.reserve(params_.joints.size());
  for (const auto & joint : params_.joints)
  {
    command_interfaces_config.names.push_back(joint + "/" + params_.command_interfaces[0]);
  }

  return command_interfaces_config;
}

controller_interface::InterfaceConfiguration LqrVmcController::state_interface_configuration() const
{
  controller_interface::InterfaceConfiguration state_interfaces_config;
  state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;

  state_interfaces_config.names.reserve(state_joints_.size());
  for (const auto & joint : state_joints_)
  {
    for (const auto & interface_type : params_.state_interfaces)
    {
      state_interfaces_config.names.push_back(joint + "/" + interface_type);
    }
  }

  return state_interfaces_config;
}

CallbackReturn LqrVmcController::on_activate(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  // TODO(anyone): if you have to manage multiple interfaces that need to be sorted check
  // `on_activate` method in `JointTrajectoryController` for exemplary use of
  // `controller_interface::get_ordered_interfaces` helper function

  // Set default value in command
  //reset_controller_reference_msg(*(input_ref_.readFromRT)());

  // 控制器非停止，订阅者激活
  is_halted = false;
  subscriber_is_active_ = true;

  return CallbackReturn::SUCCESS;
}

CallbackReturn LqrVmcController::on_deactivate(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`,
  // instead of a loop
  for (size_t i = 0; i < command_interfaces_.size(); ++i)
  {
    command_interfaces_[i].set_value(std::numeric_limits<double>::quiet_NaN());
  }


  subscriber_is_active_ = false;
  if (!is_halted)
  {
    is_halted = true;
  }
  return CallbackReturn::SUCCESS;
}

controller_interface::return_type LqrVmcController::update(
  const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
{
  // auto current_ref = input_ref_.readFromRT();

  // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`,
  // instead of a loop
  // for (size_t i = 0; i < command_interfaces_.size(); ++i)
  // {
  //   if (!std::isnan((*current_ref)->displacements[i]))
  //   {
  //     if (*(control_mode_.readFromRT()) == control_mode_type::SLOW)
  //     {
  //       (*current_ref)->displacements[i] /= 2;
  //     }
  //     command_interfaces_[i].set_value((*current_ref)->displacements[i]);

  //     (*current_ref)->displacements[i] = std::numeric_limits<double>::quiet_NaN();
  //    }
  // }

  // if (state_publisher_ && state_publisher_->trylock())
  // {
  //   state_publisher_->msg_.header.stamp = time;
  //   state_publisher_->msg_.set_point = command_interfaces_[CMD_MY_ITFS].get_value();
  //   state_publisher_->unlockAndPublish();
  // }

  auto logger = get_node()->get_logger();
  // if (get_state().id() == State::PRIMARY_STATE_INACTIVE) 
  // {
  //   if (!is_halted)
  //   {
  //     is_halted = true;
  //   }
  //   return controller_interface::return_type::OK;
  // }

  // 检查控制模式
  if (*(control_mode_.readFromRT()) == control_mode_type::STOP)
  {
    if (!is_halted)
    {
      is_halted = true;
    }
    // 控制力矩归零
    command_interfaces_[right_thigh_link_joint_effort].set_value(0.0);
    command_interfaces_[right_wheel_link_joint_effort].set_value(0.0);
    command_interfaces_[left_thigh_link_joint_effort].set_value(0.0);
    command_interfaces_[left_wheel_link_joint_effort].set_value(0.0);

    return controller_interface::return_type::OK;
  }
  else if (*(control_mode_.readFromRT()) == control_mode_type::RUNNING)
  {
    is_halted = false;
  }
  

  std::shared_ptr<ControllerReferenceMsg> last_command_msg;
  received_velocity_msg_ptr_.get(last_command_msg);
  if (last_command_msg == nullptr)
  {
    RCLCPP_WARN(logger, "Velocity message received was a nullptr.");
    return controller_interface::return_type::ERROR;
  }

  ControllerReferenceMsg command = *last_command_msg;
  // 超时刹车
  auto age_of_last_command = time - last_command_msg->header.stamp;
  // Brake if cmd_vel has timeout, override the stored command

  // 超时没有接收到速度指令就会停止
  rclcpp::Duration timeout(cmd_vel_timeout_);
  if (age_of_last_command >= timeout)
  {
    last_command_msg->twist.linear.x = 0.0;
    last_command_msg->twist.angular.z = 0.0;
  }

  // 上层速度指令
  double & linear_command = command.twist.linear.x; 
  double & angular_command = command.twist.angular.z;

  // 记录更新周期时间
  auto update_duration = (time - previous_update_timestamp_).seconds();
  previous_update_timestamp_ = time;

  // Apply (possibly new) multipliers:
  const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation;
  const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius;
  const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius;
  odometry_.setWheelParams(wheel_separation, left_wheel_radius, right_wheel_radius);

  // odom开环更新
  odometry_.updateOpenLoop(linear_command, angular_command, time);

  tf2::Quaternion orientation;
  orientation.setRPY(0.0, 0.0, odometry_.getHeading());

  // 判断是否应该发布 /odom /tf 
  bool should_publish = false;
  try
  {
    if (previous_publish_timestamp_ + publish_period_ < time)
    {
      previous_publish_timestamp_ += publish_period_;
      should_publish = true;
    }
  }
  catch (const std::runtime_error &)
  {
    // Handle exceptions when the time source changes and initialize publish timestamp
    previous_publish_timestamp_ = time;
    should_publish = true;
  }

  if (should_publish)
  {
    // odom发布
    if (state_publisher_->trylock())
    {
      auto & odometry_message = state_publisher_->msg_;
      odometry_message.header.stamp = time;
      odometry_message.pose.pose.position.x = odometry_.getX();
      odometry_message.pose.pose.position.y = odometry_.getY();
      odometry_message.pose.pose.orientation.x = orientation.x();
      odometry_message.pose.pose.orientation.y = orientation.y();
      odometry_message.pose.pose.orientation.z = orientation.z();
      odometry_message.pose.pose.orientation.w = orientation.w();
      odometry_message.twist.twist.linear.x = odometry_.getLinear();
      odometry_message.twist.twist.angular.z = odometry_.getAngular();
      state_publisher_->unlockAndPublish();
    }

    // tf发布
    if (realtime_odometry_transform_publisher_->trylock())
    {
      auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front();
      transform.header.stamp = time;
      transform.transform.translation.x = odometry_.getX();
      transform.transform.translation.y = odometry_.getY();
      transform.transform.rotation.x = orientation.x();
      transform.transform.rotation.y = orientation.y();
      transform.transform.rotation.z = orientation.z();
      transform.transform.rotation.w = orientation.w();
      realtime_odometry_transform_publisher_->unlockAndPublish();
    }

  }

  // 计算机器人位移
  double rightwheelrad = state_interfaces_[right_wheel_link_joint_position].get_value();
  double leftwheelrad = state_interfaces_[left_wheel_link_joint_position].get_value();
  double robot_displacement = -(rightwheelrad + leftwheelrad)*(params_.wheel_radius)/2 ;
  displacement_inte_ = displacement_inte_ + linear_command*(1.0/updata_rate_);
  robot_displacement = robot_displacement - displacement_inte_;

  // 计算机器人速度
  double rightwheelvel = state_interfaces_[right_wheel_link_joint_velocity].get_value();
  double leftwheelvel = state_interfaces_[left_wheel_link_joint_velocity].get_value();
  double robot_velocity = -(rightwheelvel + leftwheelvel)*(params_.wheel_radius)/2;
  // 速度进行一阶低通滤波
  double alpha = 0.05;
  robot_velocity = alpha*robot_velocity + (1 - alpha)*robot_velocity_previous_;
  robot_velocity_previous_ = robot_velocity;
  robot_velocity = robot_velocity - linear_command;
  //RCLCPP_INFO(get_node()->get_logger(), "%f",linear_command);

  // 得到左右大腿电机位置
  double right_thigh_theta = state_interfaces_[right_thigh_link_joint_position].get_value();
  double left_thigh_theta = state_interfaces_[left_thigh_link_joint_position].get_value();
  // RCLCPP_INFO(get_node()->get_logger(), "%f, %f/n",right_thigh_theta, left_thigh_theta);

  // 计算机器人Pitch
  std::shared_ptr<sensor_msgs::msg::Imu> imu_msg_ptr;
  received_imu_msg_ptr_.get(imu_msg_ptr);
  auto imu_msg =  *imu_msg_ptr; 
  tf2::Quaternion imu_orientation;
  tf2::fromMsg(imu_msg.orientation, imu_orientation);
  tf2::Matrix3x3 m(imu_orientation);
  double roll, pitch, yaw;
  m.getRPY(roll, pitch, yaw);
  double robot_pitch = + pitch; // 实物需修改为 -pitch
  // 添加质心初始偏角补偿
  double initial_pitch = lqr_vmc_.initial_pitch(right_thigh_theta, left_thigh_theta);
  robot_pitch += initial_pitch; 
  //RCLCPP_INFO(get_node()->get_logger(), "%f, %f, %f, %f",imu_msg.orientation.x, imu_msg.orientation.y, imu_msg.orientation.z, imu_msg.orientation.w);
  //RCLCPP_INFO(get_node()->get_logger(), "%f",robot_pitch);
  //RCLCPP_INFO(logger, "roll: %f", roll);

  //对接收的roll轴姿态角度作处理  实物需注释掉
  if (roll > 0.0 )
  {
    roll -= M_PI;
  }
  else if (roll < 0.0)
  {
    roll += M_PI;
  }

  // pitch角安全判断
  if (robot_pitch >= M_PI_2 || robot_pitch <= -M_PI_2)
  {
    // 控制力矩归零
    command_interfaces_[right_thigh_link_joint_effort].set_value(0.0);
    command_interfaces_[right_wheel_link_joint_effort].set_value(0.0);
    command_interfaces_[left_thigh_link_joint_effort].set_value(0.0);
    command_interfaces_[left_wheel_link_joint_effort].set_value(0.0);

    return controller_interface::return_type::ERROR;
  }
  // 将局部角速度转换为欧拉角速度微分
  double zyxPitchome = cos(roll)*imu_msg.angular_velocity.y - sin(roll)*imu_msg.angular_velocity.z;
  double zyxYawome = (sin(roll)/cos(pitch))*imu_msg.angular_velocity.y + (cos(roll)/cos(pitch))*imu_msg.angular_velocity.z;

  // 计算机器人Pitchome
  double robot_pitchome = -zyxPitchome;

  // 计算机器人Yaw
  current_yaw_inte_ = current_yaw_inte_ + (-zyxYawome*(1.0/updata_rate_));
  double robot_yaw = current_yaw_inte_;
  yaw_inte_ = yaw_inte_ + angular_command*(1.0/updata_rate_);
  robot_yaw = robot_yaw - yaw_inte_;
  //RCLCPP_INFO(get_node()->get_logger(), "%f",robot_yaw);

  // 计算机器人Yawome
  double robot_yawome = -zyxYawome - angular_command;

  /* 计算左右两轮输出扭矩 */
  lqr_vmc_.setlqrfeedback(robot_displacement, robot_pitch, robot_velocity, robot_pitchome, robot_yaw, robot_yawome);
  Eigen::Matrix<double,2,1> wheels_effort;
  wheels_effort = lqr_vmc_.lqr(right_thigh_theta, left_thigh_theta);
  // 矩阵第一个元素为左轮扭矩，第二个元素为右轮扭矩
  if (-wheels_effort(0, 0) >= 50.0)
  {
     command_interfaces_[left_wheel_link_joint_effort].set_value(50.0);
  }
  else if(-wheels_effort(0, 0) <= -50.0)
  {
    command_interfaces_[left_wheel_link_joint_effort].set_value(-50.0);
  }
  else
  {
    command_interfaces_[left_wheel_link_joint_effort].set_value(-wheels_effort(0, 0));
  }

  if (-wheels_effort(1, 0) >= 50.0)
  {
     command_interfaces_[right_wheel_link_joint_effort].set_value(50.0);
  }
  else if(-wheels_effort(1, 0) <= -50.0)
  {
    command_interfaces_[right_wheel_link_joint_effort].set_value(-50.0);
  }
  else
  {
    command_interfaces_[right_wheel_link_joint_effort].set_value(-wheels_effort(1, 0));
  }
  
  //RCLCPP_INFO(get_node()->get_logger(), "%f, %f/n",-wheels_effort(0,0), -wheels_effort(1,0));

  /* 计算左右大腿输出扭矩 */
  if (rollangle_fivethplanner_.isplanning())
  {
    roll_angle_follow_ = rollangle_fivethplanner_.getPosition();
  }
  double roll_PD_L = lqr_vmc_.roll_PD(roll_angle_follow_, roll);
  //RCLCPP_INFO(logger, "roll: %f",roll);

  if (thighangle_fivethplanner_.isplanning())
  {
    thigh_angle_follow_ = thighangle_fivethplanner_.getPosition();
  }
  double right_thigh_effort = lqr_vmc_.vmc_right(thigh_angle_follow_, right_thigh_theta, -roll_PD_L, roll_angle_follow_, roll);
  double left_thigh_effort = lqr_vmc_.vmc_left(thigh_angle_follow_, left_thigh_theta, roll_PD_L, roll_angle_follow_, roll);
  // 力矩限制
  if (right_thigh_effort >= 110.0)
  {
    command_interfaces_[right_thigh_link_joint_effort].set_value(110.0);
  }
  else if (right_thigh_effort <= -110.0)
  {
    command_interfaces_[right_thigh_link_joint_effort].set_value(-110.0);
  }
  else
  {
    command_interfaces_[right_thigh_link_joint_effort].set_value(right_thigh_effort);
  }
  if (left_thigh_effort >= 110.0)
  {
    command_interfaces_[left_thigh_link_joint_effort].set_value(110.0);
  }
  else if (left_thigh_effort <= -110.0)
  {
    command_interfaces_[left_thigh_link_joint_effort].set_value(-110.0);
  }
  else
  {
    command_interfaces_[left_thigh_link_joint_effort].set_value(left_thigh_effort);
  }
  
  // RCLCPP_INFO(get_node()->get_logger(), "%f, %f/n",right_thigh_effort, left_thigh_effort);

  // debug数据发布
  if (realtime_debug_publisher_->trylock())
  {
    auto & debug_msg = realtime_debug_publisher_->msg_;
    debug_msg.stamp = time;
    debug_msg.pitch = robot_pitch;
    debug_msg.left_wheel_effort = -wheels_effort(0, 0);
    debug_msg.right_wheel_effort = -wheels_effort(1, 0);
    debug_msg.left_motor_iq = 0.0;
    debug_msg.right_motor_iq = 0.0;
    debug_msg.update_duration = update_duration;
    debug_msg.robot_velocity = robot_velocity + linear_command;
    debug_msg.robot_position = robot_displacement + displacement_inte_;
    debug_msg.pitchome = robot_pitchome;
    debug_msg.robot_velocity_target = linear_command;
    debug_msg.yawome = robot_yawome + angular_command;
    debug_msg.yawome_target = angular_command;
    debug_msg.left_thigh_effort_feedback = state_interfaces_[left_thigh_link_joint_effort_feedback].get_value();
    debug_msg.right_thigh_effort_feedback = state_interfaces_[right_thigh_link_joint_effort_feedback].get_value();
    debug_msg.left_thigh_effort = left_thigh_effort;
    debug_msg.right_thigh_effort = right_thigh_effort;
    debug_msg.thigh_angle = (right_thigh_theta + left_thigh_theta)/2;
    debug_msg.thigh_angle_d = thigh_angle_follow_;
    debug_msg.roll = roll;
    debug_msg.roll_d = roll_angle_follow_;
    realtime_debug_publisher_->unlockAndPublish();
  }

  //RCLCPP_INFO(get_node()->get_logger(), "%f, %f, %f, %f, %f, %f\n",-wheels_effort(0,0), -wheels_effort(1,0), right_thigh_theta, left_thigh_theta, robot_displacement , robot_pitch);

  return controller_interface::return_type::OK;
}

}  // namespace lqr_vmc_controller

#include "pluginlib/class_list_macros.hpp"

PLUGINLIB_EXPORT_CLASS(
  lqr_vmc_controller::LqrVmcController, controller_interface::ControllerInterface)
